#include "Mapping.h"
#include<iostream>//
#include<pcl/io/io.h>
#include<pcl/io/pcd_io.h>//
#include <pcl/filters/voxel_grid.h>
#include<pcl/registration/icp.h>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/registration/icp.h>
#include<pcl/common/transforms.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<fstream>
#include<ctime>
#include"Config.h"
#include "comprocess.h"



Mapping::Mapping()
{
}


Mapping::~Mapping()
{
}

bool Mapping::mapDataInit(int _iterate)
{
	if (_iterate==0) {
		this->_iterate = 10;
	}
	this->_iterate = _iterate;
	return true;
}

double Mapping::getScore(pcl::PointCloud<PointIoType>::Ptr tar_pc, pcl::PointCloud<PointIoType>::Ptr sour_pc)
{
	double fitness_score = 0.0;
	double max_range = std::numeric_limits<double>::max();
	std::vector<int> nn_indices(1);
	std::vector<float> nn_dists(1);
	pcl::KdTreeFLANN<PointIoType> _Kdtree;
	_Kdtree.setInputCloud(tar_pc);
	//pcl::transformPointCloud(*sour_pc, *sour_pc, transformation_matrix);
	// For each point in the source dataset
	int nr = 0;
	for (size_t i = 0; i < sour_pc->points.size(); ++i)
	{
		// Find its nearest neighbor in the target
		_Kdtree.nearestKSearch(sour_pc->points[i], 1, nn_indices, nn_dists);

		// Deal with occlusions (incomplete targets)
		if (nn_dists[0] <= max_range)
		{
			// Add to the fitness score
			fitness_score += nn_dists[0];
			nr++;
		}
	}
	if (nr > 0)
		my_score = (fitness_score / nr);
	else
		my_score = std::numeric_limits<double>::max();
	if (nr > 0)
		return (fitness_score / nr);
	else
		return (std::numeric_limits<double>::max());
	
}

double Mapping::getScore()
{
	return my_score;
}

double Mapping::getScore(pcl::PointCloud<PointIoType>::Ptr pc_in, const Eigen::Matrix4d transformation_matrix)
{
	double fitness_score = 0.0;
	double max_range = std::numeric_limits<double>::max();
	std::vector<int> nn_indices(1);
	std::vector<float> nn_dists(1);
	pcl::KdTreeFLANN<PointIoType> _Kdtree;
	_Kdtree.setInputCloud(pc_in);
	// Transform the input dataset using the final transformation
	pcl::PointCloud<PointIoType>::Ptr sour_pc(new pcl::PointCloud<PointIoType>);
	pcl::transformPointCloud(*pc_in,*sour_pc, transformation_matrix);

	// For each point in the source dataset
	int nr = 0;
	for (size_t i = 0; i < sour_pc->points.size(); ++i)
	{
		// Find its nearest neighbor in the target
		_Kdtree.nearestKSearch(sour_pc->points[i], 1, nn_indices, nn_dists);

		// Deal with occlusions (incomplete targets)
		if (nn_dists[0] <= max_range)
		{
			// Add to the fitness score
			fitness_score += nn_dists[0];
			nr++;
		}
	}

	if (nr > 0)
		return (fitness_score / nr);
	else
		return (std::numeric_limits<double>::max());
}

Eigen::Matrix4d Mapping::getTransformation()
{
	return transformation_matrix;
}

void Mapping::registeICP(pcl::PointCloud<PointIoType>::Ptr tar_pc,
	pcl::PointCloud<PointIoType>::Ptr sour_pc,
	pcl::PointCloud<PointIoType>::Ptr output)
{
	

	// The Iterative Closest Point algorithm
	pcl::IterativeClosestPoint<PointIoType, PointIoType> icp;
	icp.setMaximumIterations(_iterate);

	icp.setInputSource(sour_pc);
	icp.setInputTarget(tar_pc);
	icp.setMaximumIterations(_iterate+5);
	icp.align(*output);
	if (icp.hasConverged())
	{
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\nICP transformation " << _iterate << " : cloud_icp -> cloud_in" << std::endl;
		transformation_matrix = icp.getFinalTransformation().cast<double>();
		//return getScore(tar_pc, output);
		my_score = icp.getFitnessScore();
		print4x4Matrix(transformation_matrix);
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
	}
}

void Mapping::registeNDT(pcl::PointCloud<PointIoType>::Ptr tar_pc,
	pcl::PointCloud<PointIoType>::Ptr sour_pc,
	pcl::PointCloud<PointIoType>::Ptr output)
{

}

void Mapping::buildGroundTruthMapToBin(const string bin_input_path)
{
	int distance_threshold = 25;
	int frame_num = 100;
	Comprocess mComprocess;
	vector<Eigen::Matrix4d> transform;
	vector<string> bin_filename;
	bin_filename = mComprocess.listFiles(bin_input_path.data());
	string inputfilename;
	mComprocess.transfromMatrix_Reader(transform, bin_input_path);

	int32_t out_num = 1000000 * frame_num;
	//float *map_data = (float*)malloc(out_num * sizeof(float));
	//float *mpx = map_data + 0;
	//float *mpy = map_data + 1;
	//float *mpz = map_data + 2;
	//float *mpr = map_data + 3;
	string outputfilename = "Map.bin";
	outputfilename = outputfilename.insert(0, bin_input_path);
	FILE *out_stream;
	fopen_s(&out_stream, outputfilename.data(), "ab+");

	int after_erase_num = 0;
	float *after_cut = (float*)malloc(out_num * sizeof(float));
	float *global = after_cut;
	for (int i = 0; i < 1100; i += 10) {
		after_erase_num = 0;
		inputfilename = bin_filename[i];
		inputfilename = inputfilename.insert(0, bin_input_path);
		const char* inptfile = inputfilename.data();
		int32_t num = 1000000;
		float *data = (float*)malloc(num * sizeof(float));
		float *px = data + 0;
		float *py = data + 1;
		float *pz = data + 2;
		float *pr = data + 3;
		FILE *stream;
		fopen_s(&stream, inptfile, "rb");
		num = fread(data, sizeof(float), num, stream) / 4;//
		fclose(stream);
		float *px_t = px;
		float *py_t = py;
		float *pz_t = pz;
		float *pr_t = pr;//
		for (int k = 0; k < num; k++)
		{
			//setting a threshold according to the distance between the point and centre to decrease the points 
			double distance = sqrt((*px_t)*(*px_t) + (*py_t)*(*py_t) + (*pz_t)*(*pz_t));
			if (distance < distance_threshold) {
				after_erase_num += 1;
			}
			px_t += 4; py_t += 4; pz_t += 4; pr_t += 4;
		}

		after_cut = data;
		float *px_s = after_cut;
		float *py_s = after_cut + 1;
		float *pz_s = after_cut + 2;
		float *pr_s = after_cut + 3;
		float tpx;
		float tpy;
		float tpz;
		cout << transform[i] << endl;

		for (int32_t j = 0; j < after_erase_num; j++)
		{
			double distance = sqrt((*px)*(*px) + (*py)*(*py) + (*pz)*(*pz));
			if (distance < distance_threshold) {
				tpx = *px;
				tpy = *py;
				tpz = *pz;
				*px_s = transform[i](0, 0) * tpx + transform[i](0, 1) * tpy + transform[i](0, 2) * tpz + transform[i](0, 3);
				*py_s = transform[i](1, 0) * tpx + transform[i](1, 1) * tpy + transform[i](1, 2) * tpz + transform[i](1, 3);
				*pz_s = transform[i](2, 0) * tpx + transform[i](2, 1) * tpy + transform[i](2, 2) * tpz + transform[i](2, 3);
				*pr_s = *pr;
				px_s += 4; py_s += 4; pz_s += 4; pr_s += 4;
			}
			px += 4; py += 4; pz += 4; pr += 4;
		}
		fwrite(after_cut, sizeof(float), 4 * after_erase_num, out_stream);
		after_cut += 16 * after_erase_num;
		cout << "process the " << i << "th pointcloud" << endl;
	}
	fclose(out_stream);


}

void Mapping::buildGroundTruthMapToPCD(const string pcd_input_path)
{
	Comprocess mComprocess;
	//bin2pcd if you need 
	/*char bin_inpput_path[200] = BIN_INPUT_PATH;
	if (mComprocess.executebin2pcd(bin_inpput_path)) {
		return 0;
	};*/
	//if you use the code bin2pcd above,please annotation the remaining code below
	vector<Eigen::Matrix4d> transform;
	vector<string> pcd_filename;
	mComprocess.transfromMatrix_Reader(transform, pcd_input_path);
	if (transform.empty()) {
		return;
	}
	pcd_filename = mComprocess.listFiles(pcd_input_path.data());
	if (pcd_filename.empty()) {
		return;
	}
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer"));
	pcl::PointCloud<PointIoType>::Ptr map(new pcl::PointCloud<PointIoType>());
	pcl::PointCloud<PointIoType>::Ptr target(new pcl::PointCloud<PointIoType>());
	pcl::PointCloud<PointIoType>::Ptr source(new pcl::PointCloud<PointIoType>());
	string load_path;
	for (int i = 0; i < 1100; i += 10) {
		load_path = pcd_input_path + pcd_filename[i];
		//load pcd to a pointcloud name map
		if (pcl::io::loadPCDFile(load_path, *source) == -1)
			return;
		//transform the pointcloud pose according the transform to the cloud_transfrom

		pcl::transformPointCloud(*source, *target, transform[i]);

		//add the cloud_transfrom to map 
		*map = *map + *target;
		cout << "process the " << i << "th pointcloud" << endl;
	}
	//down sampling the points cloud to decrease the memeory
	pcl::VoxelGrid<pcl::PointXYZ> filter;
	filter.setInputCloud(map);
	filter.setLeafSize(0.1f, 0.1f, 0.1f);
	filter.filter(*map);
	//save the map
	pcl::io::savePCDFileASCII(pcd_input_path, *map);
	//show the map
	viewer1->addPointCloud<PointIoType>(map, "map");
	viewer1->removePointCloud("source");
	viewer1->setBackgroundColor(0, 0, 0);
	while (!viewer1->wasStopped())
	{
		viewer1->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	
}
void Mapping::print4x4Matrix(const Eigen::Matrix4d & matrix)
{
	printf("Rotation matrix :\n");
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}
